%=============================== lynx6_gui ===============================
%
%  This is a GUI function for interfacing playing around with the
%  Lynxmotion L6 manipulator.
%
%=============================== lynx6_gui ===============================

%
%  Name:	lynx6_gui.m
%
%  Author:	Patricio A. Vela, pvela@gatech.edu
%
%  Created:	2008/10/16
%  Modified:	2008/10/16
%
%  Version:	0.0.1
%
%=============================== lynx6_gui ===============================
function lynx6_gui


%---(1) Setup the interface.
xsize = 900;
ysize = 500;
pfh = imfigProgramMA([], [ysize xsize], 1, [0.025 0.025 0.25 0.025], ...
                     900, [100, -50]);


pfh.addControl('quit','Style','pushbutton','String','Quit', ...
               'Position',[xsize-40,10,60,20],...
	       'TooltipString','Push to quit.', ...
               'Callback',{@quitCallback});

pfh.addControl('connect','Style','pushbutton','String','Connect', ...
               'Position',[20,10,60,20],...
	       'TooltipString','Push to connect to SRV-1 robot.', ...
               'Callback',{@connectCallback});

pfh.addControl('disconnect','Style','pushbutton','String','Disconnect', ...
               'Enable', 'off', 'Position',[100,10,60,20],...
	       'TooltipString','Push to disconnect from SRV-1 robot.', ...
               'KeyPressFcn',{@keypressCallback}, ...
               'Callback',{@disconnectCallback});

pfh.set('KeyPressFcn',{@keypressCallback});

pfh.addAxes('top', [50 75 550 550], {'view(0,90);'});
pfh.addAxes('side',[650 375 250 250], {'view(90,0)'});
pfh.addAxes('askew',[650 75 250 250], {'view(75,35)'});

getimg = getImage;

%--(2) Setup the command variables.
isConnected = false;
ro = roachLeg();
zdefault = 0;
maxrad = 3;
xpick = [-2 ; 3.5; zdefault];
xplace = [-2 ;2.5; zdefault];
alpha = [0;0;0;0;0];


%--(3) Run the system.

doQuit = false;
while (~doQuit)
  I = getimg();
  displayAll();
  drawnow();
  pause(0.1);
end


pfh.free();
pfh = [];


%======================== User Interface Functions =======================

  %---------------------------- getSRVImage ----------------------------
  %
  %  Grab an image from the SRV-1.
  %
  function I = getImage
  I = [];
  end

  %
  %----------------------------- displayAll ----------------------------
  %
  %  Ignoring I argument for now.
  %
  function displayAll(I)
  fignames = {'top','side','askew'};
  for ii=1:length(fignames)
    pfh.currentAxes(fignames{ii});
    ro.roachdisplay(alpha);
    pfh.prettyAxes(fignames{ii});
    hold on;
    if (~isempty(xpick))
      plot3(xpick(1), xpick(2), xpick(3), 'ro', 'MarkerSize',5);
    end
    if (~isempty(xplace))
      plot3(xplace(1), xplace(2), xplace(3), 'rx', 'MarkerSize',5);
    end
    hold off;
  end

  end

  %
  %--------------------------- inputStartPoint --------------------------
  %
  %
  %
  function inputStartPoint
  pfh.message('Enter the location of the object to grab.');
  pfh.currentAxes('top');
  [x, y] = ginput(1);
  %Test for validity.
  xpick = [x; y; zdefault];
  if (sqrt(x^2+y^2) > maxrad)
    xpick(1:2) = xpick(1:2)*maxrad/norm(xpick(1:2));
  end
  end
  
  %
  %-------------------------- inputEndPoint --------------------------
  %
  %
  %
  function inputEndPoint
  pfh.message('Enter the location to place the object.');
  pfh.currentAxes('top');
  [x, y] = ginput(1);
  %Text for validity.
  xplace = [x; y; zdefault];
  if (sqrt(x^2+y^2) > maxrad)
    xplace(1:2) = xplace(1:2)*maxrad/norm(xplace(1:2));
  end
  end

  %
  %--------------------------- execPlacement ---------------------------
  %
  %
  %
  function execPlacement
  if (isempty(xpick) || isempty(xplace) || ~isConnected)
    return;
  end
  pfh.message('Executing the desired pick-n-place operation.');
  disp('Go for it');

 Rdes   = [1, 0, 0;0, -1, 0; 0, 0, -1];
%   Rdes   = [1, 0, 0; 0, 0, 1; 0, -1, 0];
  gpick  = [Rdes , xpick ; 0 0 0 1];
  gplace = [Rdes , xplace; 0 0 0 1];


  alphaStart = [ro.inversekin(gpick).'];
  alphaFinal = [ro.inversekin(gplace).'];
  alphaDes = [alphaStart, alphaFinal,alphaStart];
  times = [1, 1];
%   dtraj = spline([0, cumsum(times)], alphaDes);
%   alphatraj = @(t)ppval(dtraj,t);
%   dtraj = spaps([0, cumsum(times)], alphaDes, 1000);
%   alphatraj = @(t)fnval(dtraj,t);
  alphatraj = @(t)interp1([0, cumsum(times)], alphaDes', t)';
%   figure(2);
%   plot([0 cumsum(times)], alphaDes);
%   legend('1','2','3','4','5','6');
%   figure(3);
%   plot([0:0.1:sum(times)], alphatraj([0:0.1:sum(times)]));
  figure(1);
  tstep = 0.1;
  for i=0:tstep:sum(times)
    alpha = alphatraj(i);
%     l6.setArm(alpha, tstep);
    displayAll();
    drawnow();
    pause(0.01);
  end 

  end
  %
  %-------------------------- keypressCallback -------------------------
  %
  %  Function to handle keypresses.
  %
  function keypressCallback(source, eventdata)
  switch (eventdata.Character)
    case {'G','g'}
      inputStartPoint();
    case {'P','p'}
      inputEndPoint();
    case {'X','x'}
      execPlacement();
    case {'Q','q'}
      doQuit = true;
    %otherwise
      %source
      %eventdata
  end
  end

  %-------------------------- connectCallback --------------------------
  %
  %  Function to handle the "connect" button press.
  function connectCallback(source, eventdata)
%   for ii = 1:5
%     l6.gotoSleep();
%     pause(0.05);
%   end
%   l6.gotoHome();
  pfh.modControl('disconnect','Enable','on');
  pfh.modControl('connect','Enable','off');
  pfh.set('KeyPressFcn',{@keypressCallback});
  isConnected = true;
  %TODO: Also need to disable the button.
  %TODO: then enable the disconnect button.
  end

  function disconnectCallback(source, eventdata)
%   l6.gotoSleep();
%   l6.shutdown();
%   pause(1);
%   l6 = [];
  pfh.modControl('connect','Enable','on');
  pfh.modControl('disconnect','Enable','off');
  pfh.set('KeyPressFcn',[]);
  isConnected = false;
  end

  function quitCallback(source, eventdata)
  doQuit = true;
  end


end


%----------------------------- getFakeImage ----------------------------
%
%  For testing when robot is not connected.  Grabs a fake image.
%
function I = getFakeImage
I = zeros([240 320]);
I(10:20,10:20) =  50;
I(110:120,110:120) =  150;
end

